
#include "board.h"
#include "sensor/mpu6050.h"
#include <stdint.h>
#include <stdio.h>

int main_test(void)
{
	int ret = 0;
	float pitch;
	float roll;
	float yaw;
	int p = 0;
	int r = 0;
	int y = 0;
	mpu6050_t sensor;

	board_init();

	sensor.cfg.fix.device_address = 0x68;
	sensor.ops.i2c_read = board_i2c_read;
	sensor.ops.i2c_write = board_i2c_write;

	ret = mpu6050_init(&sensor);
	if (ret) {
		printf("err: mpu6050_init(), ret = %d\r\n", ret);
	}

	while (1) {
		ret = mpu6050_read(&pitch, &roll, &yaw);
		if (ret) {
			printf("err: mpu6050_read(), ret = %d\r\n", ret);
			delay_ms(10);
			continue;
		}

		p = (int)pitch;
		r = (int)roll;
		y = (int)yaw;

		printf("pitch=%d, roll=%d, yaw=%d\r\n", p, r, y);
		delay_ms(10);
	}
}
